#pragma once
#include <stdio.h>
#include <stack>
#include <queue>
#include <concurrent_queue.h>
#include <omp.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/tuple/tuple.hpp>

#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>

#include "System/Platform.h"

namespace PCS
{
	namespace Core
	{
		class PointCloudPatch;		

		/**Represents, creates and manages the spatial relationships between different point cloud patches*/
		class SegmentationGraph
		{
		private:
			struct pcp_neighbour{
				Point * point_pcp1;
				Point * point_pcp2;
			};

			SparseGrid * m_pGrid;
			std::vector<PointCloudPatch*>* nodes; //Edge information is stored in the nodes themselves .. first patch in nodes is the start node.
			std::vector<std::vector<Point*>*>* patches;

		public:			
			SegmentationGraph(void) {  }
			SegmentationGraph(SparseGrid * &p_pGrid) { m_pGrid = p_pGrid; }  
			
			void Build() {
				boost::timer timer;
				boost::interprocess::file_mapping m_file(FileNameQBuffer, boost::interprocess::read_write);
				boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);
				size_t * r_addr = (size_t*)region.get_address();				
				size_t r_size = region.get_size();				
				std::cout << "[SegmentationGraph] Memory Mapped File loaded to memory region. Size= " << r_size << " Time=" <<timer.elapsed() <<std::endl;

				int numberOfProcs = omp_get_num_procs();
				std::cout << "[SegmentationGraph] " << numberOfProcs << " threads starting to grow patches" << std::endl;				
				numberOfProcs = 1;
				std::vector<size_t> * seeds = new std::vector<size_t>();
				std::map<grid_key, std::vector<size_t>*> * map = m_pGrid->GetMap();
				PointCloud * pointCloud = m_pGrid->GetPointCloud();
				int map_size = map->size();
				int step = (int) (map_size / (numberOfProcs + 0.5f));
				int crtCell = 0;

				Vector3 extent = m_pGrid->GetExtent();				
				Vector3 size = m_pGrid->GetSize();
				AxisAlignedBoundingBox * bounds = m_pGrid->GetBounds();
				int patch_id = 1;

				Concurrency::concurrent_queue<size_t> * same_patch_work_queue = new Concurrency::concurrent_queue<size_t>();
				Concurrency::concurrent_queue<std::pair<size_t, size_t>> * different_patch_work_queue = new Concurrency::concurrent_queue<std::pair<size_t, size_t>>();
				Concurrency::concurrent_queue<std::pair<int, int>> * neighbour_patches_queue = new Concurrency::concurrent_queue<std::pair<int, int>>();
				std::map<evidence_key, int> * edge2edgeContacts = new std::map<evidence_key, int>();
				std::map<evidence_key, int> * surface2surfaceContacts = new std::map<evidence_key, int>();
				std::map<evidence_key, int> * surface2edgeContacts = new std::map<evidence_key, int>();
				std::map<grid_key, std::vector<size_t>*>::iterator cellClosureIterator = map->begin();

				//Establish the seeds according to the number of processors
				#pragma region 
				boost::timer timer_0;
				std::cout << "[SegmentationGraph] Computing Initial Patch Seeds ... ";
				std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = map->begin();
				for (int i=0; i<numberOfProcs; i++) {
					for (int k=0; k<step; k++) cellIterator++;
					std::vector<size_t>::iterator pointIterator = cellIterator->second->begin();
					seeds->push_back(*pointIterator);
				}
				std::cout << boost::str(boost::format(" (%ds)") % timer_0.elapsed()) << std::endl;
				#pragma endregion				

				//Construct a map (size of sgrid) storing for each cell a vector with references to it's surrounding cells
				#pragma region 
				boost::timer timer_1;
				std::cout << "[SegmentationGraph] Computing Adjacency Map .. ";
				std::map<grid_key, std::vector<std::vector<size_t>*>*> cellNeighbours = std::map<grid_key, std::vector<std::vector<size_t>*>*>();				
				grid_key cellKey; int xIdxP, yIdxP, zIdxP;
				std::map<grid_key, std::vector<size_t>*>::iterator gridIterator;
				unsigned int nck_x, nck_y;
				for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = map->begin(); 
					 cellIterator != map->end(); ++cellIterator)
				{					
					//Determine which adjacent cells need to be considered and pass them to the neighbourhood query
					std::vector<std::vector<size_t>*> * neighbourCells = new std::vector<std::vector<size_t>*>();										
					cellKey = (*cellIterator).first;
					
					//first search the voxel this point is located in
					gridIterator = map->find(cellKey);
					neighbourCells->push_back((*gridIterator).second);
					
					for (int xIdx = -1; xIdx < 2; ++xIdx) {
						nck_x = cellKey + (xIdx << 20);
						for (int yIdx = -1; yIdx < 2; ++yIdx) {
							nck_y = nck_x + (yIdx << 10);
							for (int zIdx = -1; zIdx < 2; ++zIdx) {
								if (!(xIdx==0 && yIdx==0 && zIdx==0)) {									
									if ((gridIterator = map->find(nck_y+zIdx)) != map->end())	{										
										neighbourCells->push_back((*gridIterator).second);
									}	
								}
							}
						}							
					}

					cellNeighbours[cellKey] = neighbourCells;
				}

				std::cout << boost::str(boost::format(" (%ds)") % timer_1.elapsed()) << std::endl;
				#pragma endregion				

				boost::timer timer_2;				
				#pragma omp parallel for schedule(guided)	
				for (int i=0; i<numberOfProcs; i++) {					
					size_t seed = seeds->at(i);
					int work_done = 0;		
					Point * crtPoint = pointCloud->GetPointAt(seed);

					std::queue<size_t> * thread_same_patch_work_queue = new std::queue<size_t>();

					boost::timer timer_n;
					boost::timer timer_00;
					boost::timer timer_01;
					boost::timer timer_02;
					boost::timer timer_03;
					boost::timer timer_04;

					float time_consumed_00 = 0;
					float time_consumed_01 = 0;
					float time_consumed_02 = 0;
					float time_consumed_03 = 0;
					float time_consumed_04 = 0;

					float time_consumed = 0;
					float time_consumed_cq = 0;					
				
					crtPoint->pcpId = _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&patch_id), 1);

					//Calculate cell index in sparse grid
					int xIdxP = (int)floor((float)(((crtPoint->position.X - (bounds->GetMinExtent().X))  * size.X) / extent.X));
					int yIdxP = (int)floor((float)(((crtPoint->position.Y - (bounds->GetMinExtent().Y))  * size.Y) / extent.Y));
					int zIdxP = (int)floor((float)(((crtPoint->position.Z - (bounds->GetMinExtent().Z))  * size.Z) / extent.Z));

					//Determine neighbours and start region growth - initialise work queues from seeds
					#pragma region
					std::vector<size_t> neighbours = std::vector<size_t>();
					//m_pGrid->GetNeighbours(seed, *cellNeighbours[gridHash(xIdxP, yIdxP, zIdxP)], neighbours, N_FOR_CRAWL);
					m_pGrid->GetNeighbours(seed, neighbours, r_addr);
					std::vector<size_t>::iterator start = neighbours.begin();
					std::vector<size_t>::iterator end;
					if (neighbours.size() > N_FOR_CRAWL) end = neighbours.begin()+N_FOR_CRAWL;
					else end = neighbours.end();
					Point * nxtNeighbour;
					for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
						nxtNeighbour = pointCloud->GetPointAt(*pointIterator);
						if (nxtNeighbour->T == crtPoint->T) {
							if (nxtNeighbour->pcpId == -1) {
								nxtNeighbour->pcpId = crtPoint->pcpId;								
								same_patch_work_queue->push(*pointIterator);
							}
						} else {
							if (nxtNeighbour->pcpId == -1) {
								different_patch_work_queue->push(std::make_pair(seed, *pointIterator));
							}
						}
					}
					#pragma endregion

					//Work Queues initialised - all threads should now start processing them.
					std::pair<size_t, size_t> * p_seed = new std::pair<size_t, size_t>();
					
					std::map<evidence_key, int>::iterator evidenceMapsIterator0;
					std::map<evidence_key, int>::iterator evidenceMapsIterator1;
					std::map<evidence_key, int>::iterator evidenceMapsIterator2;
					evidence_key evidenceKey;
					int prevPatchId1 = 0;
					int prevPatchId2 = 0;
					int crtPatchId1 = 0;
					int crtPatchId2 = 0;

					int samePatchPair = 0;
					int diffPatchPair = 0;
					int pushedToLocalQueueCount = 0;
					int pushedToConcurrentQueueCount = 0;

					while (!same_patch_work_queue->empty() || !different_patch_work_queue->empty()) 
					{
						while (same_patch_work_queue->try_pop(seed)) 
						{
							thread_same_patch_work_queue->push(seed);
							while (!thread_same_patch_work_queue->empty()) 
							{								
								seed = thread_same_patch_work_queue->front();
								//std::cout << "T " << omp_get_thread_num() << " processing " << seed << " Size of Queue=" << thread_same_patch_work_queue->size() << std::endl;
								thread_same_patch_work_queue->pop();
								crtPoint = pointCloud->GetPointAt(seed);
								xIdxP = (int)floor((float)(((crtPoint->position.X - (bounds->GetMinExtent().X))  * size.X) / extent.X));
								yIdxP = (int)floor((float)(((crtPoint->position.Y - (bounds->GetMinExtent().Y))  * size.Y) / extent.Y));
								zIdxP = (int)floor((float)(((crtPoint->position.Z - (bounds->GetMinExtent().Z))  * size.Z) / extent.Z));							
								timer_n.restart();		
								//m_pGrid->GetNeighbours(seed, *cellNeighbours[gridHash(xIdxP, yIdxP, zIdxP)], neighbours, N_FOR_CRAWL);
								m_pGrid->GetNeighbours(seed, neighbours, r_addr);
								time_consumed += timer_n.elapsed();
								start = neighbours.begin();
								if (neighbours.size() > N_FOR_CRAWL) end = neighbours.begin()+N_FOR_CRAWL;
								else end = neighbours.end();
								for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) 
								{
									nxtNeighbour = pointCloud->GetPointAt(*pointIterator);
									if (nxtNeighbour->T == PCS::Core::unknown) continue; //skip iteration if point is of unknown type
									if (nxtNeighbour->T == crtPoint->T) 
									{
										if (nxtNeighbour->pcpId == -1) 
										{
											nxtNeighbour->pcpId = crtPoint->pcpId;
											thread_same_patch_work_queue->push(*pointIterator);
											//same_patch_work_queue->push(*pointIterator);
											pushedToLocalQueueCount++;
										} 
										else 
										{ 
											//timer_00.restart();
											if (nxtNeighbour->pcpId != crtPoint->pcpId) 
											{
												crtPatchId1 = nxtNeighbour->pcpId;
												crtPatchId2 = crtPoint->pcpId;
												//if ((crtPatchId1 != prevPatchId1) || (crtPatchId2 != prevPatchId2)) 
												//{
													nxtNeighbour->pcpId < crtPoint->pcpId ?
																			evidenceKey = evidenceHash(nxtNeighbour->pcpId, crtPoint->pcpId) :
																			evidenceKey = evidenceHash(crtPoint->pcpId, nxtNeighbour->pcpId);
																			//evidenceKey = boost::str(boost::format("%d:%d") % nxtNeighbour->pcpId % crtPoint->pcpId) :
																			//evidenceKey = boost::str(boost::format("%d:%d") % crtPoint->pcpId % nxtNeighbour->pcpId );												
													if (crtPoint->T == PCS::Core::edge) 
													{
														#pragma omp critical(e2eContacts)
														{
															if ((evidenceMapsIterator0 = edge2edgeContacts->find(evidenceKey)) == edge2edgeContacts->end())
																(*edge2edgeContacts)[evidenceKey] = 1;
															//else _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&(evidenceMapsIterator0->second)), 1);
															else evidenceMapsIterator0->second+=1;
														}
													}
													else 
													{
														#pragma omp critical(s2sContacts)
														{								
															if ((evidenceMapsIterator1 = surface2surfaceContacts->find(evidenceKey)) == surface2surfaceContacts->end()) 
																(*surface2surfaceContacts)[evidenceKey] = 1; 													
															//else _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&(evidenceMapsIterator1->second)), 1); 
															else evidenceMapsIterator1->second+=1;
														}											
													}
													prevPatchId1 = crtPatchId1;
													prevPatchId2 = crtPatchId2;
													diffPatchPair++;							
												//} 
												//else
												//{	//_InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&(evidenceMapsIterator->second)), 1);
													//evidenceMapsIterator->second+=1;
													//samePatchPair++;
												//}
											}
											//time_consumed_00+=timer_00.elapsed();
										}
									} 
									else 
									{
										//timer_01.restart();
										if (nxtNeighbour->pcpId == -1) 
										{
											//timer_02.restart();
											different_patch_work_queue->push(std::make_pair(seed, *pointIterator));
											pushedToConcurrentQueueCount++;
											//time_consumed_02+=timer_02.elapsed();
										} 
										else 
										{
											//add connectivity information											
											//timer_03.restart();
											crtPoint->T == PCS::Core::edge ?
													evidenceKey = evidenceHash(nxtNeighbour->pcpId, crtPoint->pcpId) :
													evidenceKey = evidenceHash(crtPoint->pcpId, nxtNeighbour->pcpId);
													//evidenceKey = boost::str(boost::format("%d:%d") % nxtNeighbour->pcpId % crtPoint->pcpId) :
													//evidenceKey = boost::str(boost::format("%d:%d") % crtPoint->pcpId % nxtNeighbour->pcpId );
											//time_consumed_03+=timer_03.elapsed();
											//timer_04.restart();
											#pragma omp critical(s2eContacts)
											{												
												if ((evidenceMapsIterator2 = surface2edgeContacts->find(evidenceKey)) == surface2edgeContacts->end())
													(*surface2edgeContacts)[evidenceKey] = 1;
												//else _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&(evidenceMapsIterator->second)), 1);
												else evidenceMapsIterator2->second+=1;											
											}											
											time_consumed_04+=timer_04.elapsed();
										}
										//time_consumed_01+=timer_01.elapsed();
									}
								}
								work_done++;
								if (work_done % 250000 == 0) 
									std::cout << boost::str(boost::format("[SegmentationGraph] T%d - #=%d") % omp_get_thread_num() % work_done) << " -> " << timer.elapsed() << std::endl;
								
							}
						}
						
						//check if there are any points in the different_patch_work_queue
						{
							if (different_patch_work_queue->try_pop(*p_seed)) {
								if (pointCloud->GetPointAt(p_seed->second)->pcpId == -1) {	
									pointCloud->GetPointAt(p_seed->second)->pcpId = _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&patch_id), 1);
									//std::cout << " [T" << omp_get_thread_num() << "] adding to queue - and set patch ID to " << p_seed->second->pcpId << std::endl;
								}
								same_patch_work_queue->push(p_seed->second);
							} 
							else 
							{ // we need to make sure that all parts of the cloud have been segmented (i.e. assigned a patch id)
								#pragma omp critical(closure)
								{
									bool added = false;
									while (cellClosureIterator != map->end()) {
										for (std::vector<size_t>::iterator pointIterator = cellClosureIterator->second->begin(); pointIterator != cellClosureIterator->second->end(); ++pointIterator) {
											if (pointCloud->GetPointAt(*pointIterator)->pcpId == -1) {
												pointCloud->GetPointAt(*pointIterator)->pcpId = _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&patch_id), 1);
												//std::cout << " [T" << omp_get_thread_num() << "] adding to queue - and set patch ID to " << (*pointIterator)->pcpId << std::endl;
												same_patch_work_queue->push(*pointIterator);
												added = true;
												break;
											}
										}
										if (!added) cellClosureIterator++; else break;
									}							
								}
							}
						}
					}
					//Now is the time to merge together the evidence collected in the local maps
					//std::cout << boost::str(boost::format("[SegmentationGraph] T%d timers. NQ %ds. Timer00 %ds. Timer01 %ds. Timer02 %ds. Timer03 %ds. Timer04 %ds.") % omp_get_thread_num() % time_consumed % time_consumed_00 % time_consumed_01 % time_consumed_02 % time_consumed_03 % time_consumed_04) << std::endl;
					std::cout << boost::str(boost::format("[SegmentationGraph] T%d - #=%d LocalQueue#=%d ConcQueue#=%d") % omp_get_thread_num() % work_done % pushedToLocalQueueCount % pushedToConcurrentQueueCount) << " -> " << timer.elapsed() << std::endl;
									
				} //concurrent for loop end	
				std::cout << boost::str(boost::format("[SegmentationGraph] All threads done in %ds") % timer_1.elapsed()) << std::endl;
				cout << "[SegmentationGraph] Contact MAPS ->" << " S2S[" << surface2surfaceContacts->size() << "] E2E[" << edge2edgeContacts->size() << "] S2E[" << surface2edgeContacts->size() << "]" << std::endl; 
				
				std::map<int, std::vector<Point*>*> * surfacePatches = new std::map<int, std::vector<Point*>*>();
				std::map<int, std::vector<Point*>*>::iterator surfacePatchesIterator;
				std::map<int, std::vector<Point*>*> * edgePatches = new std::map<int, std::vector<Point*>*>();
				std::map<int, std::vector<Point*>*>::iterator edgePatchesIterator;

				//Determine number of surface and edge patches
#pragma region
				int surfacePatchesCount = 0;
				int edgePatchesCount = 0;
				std::map<int, int> surfaceIds = std::map<int, int>();				
				std::map<int, int> edgeIds = std::map<int, int>();
				std::map<int, int>::iterator idsIterator;
				Point * crtPoint;
				for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = m_pGrid->GetMap()->begin(); cellIterator != m_pGrid->GetMap()->end(); ++cellIterator) {
					for (std::vector<size_t>::iterator pointIterator = cellIterator->second->begin(); pointIterator != cellIterator->second->end(); ++ pointIterator) {			
						crtPoint = pointCloud->GetPointAt(*pointIterator);
						if (crtPoint->T == surface) {
							 if ((idsIterator = surfaceIds.find(crtPoint->pcpId)) == surfaceIds.end()) {
								 surfaceIds[crtPoint->pcpId] = 1;
								 surfacePatchesCount++;
								 (*surfacePatches)[crtPoint->pcpId] = new std::vector<Point*>();
								 (*surfacePatches)[crtPoint->pcpId]->push_back(crtPoint);
							 } else { (*surfacePatches)[crtPoint->pcpId]->push_back(crtPoint); }
						 } else {
							 if ((idsIterator = edgeIds.find(crtPoint->pcpId)) == edgeIds.end()) {
								 edgeIds[crtPoint->pcpId] = 1;
								 edgePatchesCount++;
								 (*edgePatches)[crtPoint->pcpId] = new std::vector<Point*>();
								 (*edgePatches)[crtPoint->pcpId]->push_back(crtPoint);
							 } else { (*edgePatches)[crtPoint->pcpId]->push_back(crtPoint); }
						 }
					}
				} 
				cout << "[SegmentationGraph] Patch Count ->" << " S[" << surfacePatchesCount << "] E[" << edgePatchesCount << "]" << std::endl;
				//for (surfacePatchesIterator = surfacePatches->begin(); surfacePatchesIterator != surfacePatches->end(); ++surfacePatchesIterator) {
				//	std::cout << surfacePatchesIterator->first << " -> " << surfacePatchesIterator->second->size() << std::endl;			
				//}				
#pragma endregion

				std::cout << boost::str(boost::format("[SegmentationGraph] Segmentation process complete in %ds") % timer.elapsed()) << std::endl;
				std::cout << "[SegmentationGraph] Starting to merge surface patches" << std::endl;
				boost::timer timer_merging;
//Group surface ids of touching connected surfaces
#pragma region				
				std::vector<std::string> splitVec;
				std::map<int, std::vector<int>*> surfacePatchGroups = std::map<int, std::vector<int>*>();
				std::map<int, std::vector<int>*>::iterator surfacePatchGroupsIterator;
				std::map<int, std::vector<int>*>::iterator surfacePatchGroupsIterator2;
				std::map<int, int> surfaceVisitedMap = std::map<int, int>();
				std::map<int, int>::iterator surfaceVisitedMapIterator;
				std::map<int, int>::iterator surfaceVisitedMapIterator2;
				int patch1 = -1; int patch2 = -2; int patchKey = -2; int patchNotKey = -2; int pointsInContact = 0;
				for (std::map<evidence_key, int>::iterator contactsIterator = surface2surfaceContacts->begin(); 
					 contactsIterator != surface2surfaceContacts->end(); ++contactsIterator)
					{
						//std::cout << contactsIterator->first << "[" << contactsIterator->second << "]" << std::endl;
						//boost::algorithm::split(splitVec, contactsIterator->first, boost::algorithm::is_any_of(":"), boost::algorithm::token_compress_on);					
						//patch1 = atoi(splitVec[0].c_str());
						//patch2 = atoi(splitVec[1].c_str());
						unsigned int *patch = (unsigned int *) &(contactsIterator->first);
						patch1 = patch[0];
						patch2 = patch[1];
						
						pointsInContact = contactsIterator->second;

						//If the contact between two huge patches is minimal do not group them together
						//Establish whether this connection should be processed - 100 is arbitrary
						if (((*surfacePatches)[patch1]->size() > (pointsInContact * 100)) && ((*surfacePatches)[patch2]->size() > (pointsInContact * 100))) 							
							continue;	

						if ((*surfacePatches)[patch1]->size() > (*surfacePatches)[patch2]->size()) { patchKey = patch1; patchNotKey = patch2; } else { patchKey = patch2; patchNotKey = patch1; }
						//if patch1 and patch2 are already both keys then we have to merge one into the other.
						if ( (surfacePatchGroupsIterator = surfacePatchGroups.find(patchKey)) != surfacePatchGroups.end() &&
							 (surfacePatchGroupsIterator2 = surfacePatchGroups.find(patchNotKey)) != surfacePatchGroups.end() ) {							
							int pksize = surfacePatchGroups[patchKey]->size();
							int pnksize = surfacePatchGroups[patchNotKey]->size();
							//std::cout << "Both ids ARE KEYS in Different Groups " << patchKey << " size=" << pksize <<". " << patchNotKey << " size=" << pnksize << std::endl;
							if (pksize > pnksize) {
								//merge patchnotkey into patchkey group                                
								int mergegroup = patchNotKey;
								//std::cout << "Merging " << mergegroup << " group into " << patchKey << "iteration size = " << pnksize << std::endl;
								for (int i = 0; i<pnksize; ++i) {
									//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << patchKey << " group " << std::endl;
									surfacePatchGroups[patchKey]->push_back(surfacePatchGroups[mergegroup]->at(i));
									//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << patchKey << std::endl;
									surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = patchKey;
								}
								surfacePatchGroups[patchKey]->push_back(mergegroup);
								surfacePatchGroups.erase(mergegroup);
							} else {
								//merge patchkeygroup into patchnotkeygroup
								int mergegroup = patchKey;
								//std::cout << "Merging " << mergegroup << " group into " << patchNotKey << "iteration size = " << pksize << std::endl;
								for (int i = 0; i<pksize; ++i) {
									//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << patchNotKey << " group " << std::endl;
									surfacePatchGroups[patchNotKey]->push_back(surfacePatchGroups[mergegroup]->at(i));
									//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << patchNotKey << std::endl;
									surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = patchNotKey;
								}
								surfacePatchGroups[patchNotKey]->push_back(mergegroup);
								surfacePatchGroups.erase(mergegroup);
							}
							continue;
						}

						//if patch1 is already a key add patch2 to it
						if ( (surfacePatchGroupsIterator = surfacePatchGroups.find(patchKey)) != surfacePatchGroups.end() ) {
							if ((surfaceVisitedMapIterator = surfaceVisitedMap.find(patchNotKey)) == surfaceVisitedMap.end()) {
								surfacePatchGroups[patchKey]->push_back(patchNotKey);
								surfaceVisitedMap[patchNotKey] = patchKey;
								//std::cout << "Adding " << patchKey << " <-[1] " << patchNotKey << std::endl;
							} else { 
								//Check if they are in the same group ... if not we have to merge!!!
								if (surfaceVisitedMap[patchNotKey] != patchKey) {
									//std::cout << "Both keys present BUT in Different Groups " << patchKey << " is Group Key. " << patchNotKey << " is in group with key " << surfaceVisitedMap[patchNotKey] << std::endl;
									if (((*surfacePatches)[patchKey]->size()) > ((*surfacePatches)[surfaceVisitedMap[patchNotKey]]->size())) {
										//merge patchNotKey into patchKey
										int gsize = surfacePatchGroups[surfaceVisitedMap[patchNotKey]]->size();
										int mergegroup = surfaceVisitedMap[patchNotKey];
										//std::cout << "Merging " << mergegroup << " group into " << patchKey << "iteration size = " << gsize << std::endl;
										for (int i = 0; i<gsize; ++i) {
											//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << patchKey << " group " << std::endl;
											surfacePatchGroups[patchKey]->push_back(surfacePatchGroups[mergegroup]->at(i));
											//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << patchKey << std::endl;
											surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = patchKey;
										}
										surfacePatchGroups[patchKey]->push_back(mergegroup);
										surfacePatchGroups.erase(mergegroup);								
									} else { 
										//merge patchKey into patchNotKey
										int gsize = surfacePatchGroups[patchKey]->size();
										int mergegroup = patchKey;										
										//std::cout << "Merging " << mergegroup << " group into " << surfaceVisitedMap[patchNotKey] << "iteration size = " << gsize << std::endl;
										for (int i = 0; i<gsize; ++i) {
											//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchNotKey] << " group " << std::endl;
											surfacePatchGroups[surfaceVisitedMap[patchNotKey]]->push_back(surfacePatchGroups[mergegroup]->at(i));
											//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchNotKey] << std::endl;
											surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = surfaceVisitedMap[patchNotKey];
										}
										surfacePatchGroups[surfaceVisitedMap[patchNotKey]]->push_back(mergegroup);
										surfacePatchGroups.erase(mergegroup);			
									}									
								} else { //std::cout << "Both keys present AND in same group " << patchKey << " <-[Nothing Done A] -> " << patchNotKey << std::endl; 
								}
							}			
							continue;
						}

						//if patch2 is already a key add patch1 to it
						if ( (surfacePatchGroupsIterator = surfacePatchGroups.find(patchNotKey)) != surfacePatchGroups.end() ) {
							if ((surfaceVisitedMapIterator = surfaceVisitedMap.find(patchKey)) == surfaceVisitedMap.end()) {
								surfacePatchGroups[patchNotKey]->push_back(patchKey);
								surfaceVisitedMap[patchKey] = patchNotKey;
								//std::cout << "Adding " << patchNotKey << " <-[2] " << patchKey << std::endl;
							} else { 
								//Check if they are in the same group ... if not we have to merge!!!
								if (surfaceVisitedMap[patchKey] != patchNotKey) {
									//std::cout << "Both keys present BUT in Different Groups " << patchNotKey << " is Group Key. " << patchKey << " is in group with key " << surfaceVisitedMap[patchKey] << std::endl;
									if (((*surfacePatches)[surfaceVisitedMap[patchKey]]->size()) > ((*surfacePatches)[patchNotKey]->size())) {
										//merge patchNotKey into surfaceVisistedMap[patchKey]
										int gsize = surfacePatchGroups[patchNotKey]->size();
										int mergegroup = patchNotKey;
										//std::cout << "Merging " << mergegroup << " group into " << surfaceVisitedMap[patchKey] << "iteration size = " << gsize << std::endl;
										for (int i = 0; i<gsize; ++i) {
											//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchKey] << " group " << std::endl;
											surfacePatchGroups[surfaceVisitedMap[patchKey]]->push_back(surfacePatchGroups[mergegroup]->at(i));
											//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchKey] << std::endl;
											surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = surfaceVisitedMap[patchKey];
										}
										surfacePatchGroups[surfaceVisitedMap[patchKey]]->push_back(mergegroup);
										surfacePatchGroups.erase(mergegroup);								
									} else { 
										//merge patchKey into patchNotKey
										int gsize = surfacePatchGroups[surfaceVisitedMap[patchKey]]->size();
										int mergegroup = surfaceVisitedMap[patchKey];
										//std::cout << "Merging " << mergegroup << " group into " << patchNotKey << "iteration size = " << gsize << std::endl;
										for (int i = 0; i<gsize; ++i) {
											//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchKey] << " group " << std::endl;
											surfacePatchGroups[patchNotKey]->push_back(surfacePatchGroups[mergegroup]->at(i));
											//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << patchNotKey << std::endl;
											surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = patchNotKey;
										}
										surfacePatchGroups[patchNotKey]->push_back(mergegroup);
										surfacePatchGroups.erase(mergegroup);								
									}
								} else { /*std::cout << "Both keys present AND in same group " << patchNotKey << " <-[Nothing Done B]-> " << patchKey << std::endl;*/ }
							}	
							continue;
						}

						//if neither of these patches has been encountered so far ... create a new entry in surface patches
						if ( (surfaceVisitedMapIterator = surfaceVisitedMap.find(patchKey)) == surfaceVisitedMap.end() && 
							 (surfaceVisitedMapIterator2 = surfaceVisitedMap.find(patchNotKey)) == surfaceVisitedMap.end() ) {							
							surfacePatchGroups[patchKey] = new std::vector<int>();
							surfacePatchGroups[patchKey]->push_back(patchNotKey);
							surfaceVisitedMap[patchNotKey] = patchKey;
							//std::cout << "Create Patch Group ID " << patchKey << " <-[NEW] and adding " << patchNotKey << std::endl;
							continue;							
						}

						//if patch1 is on a group OR (not AND) patch2 is on a group ... first check patchKey.
						if ( ((surfaceVisitedMapIterator = surfaceVisitedMap.find(patchKey)) != surfaceVisitedMap.end()) &&
							 ((surfaceVisitedMapIterator2 = surfaceVisitedMap.find(patchNotKey)) == surfaceVisitedMap.end()) ) {
							//if (((*surfacePatches)[surfaceVisitedMap[patchKey]]->size() > (pointsInContact * 100)) && 
							//	((*surfacePatches)[patchNotKey]->size() > (pointsInContact * 100))) continue;
							surfacePatchGroups[surfaceVisitedMap[patchKey]]->push_back(patchNotKey);
							surfaceVisitedMap[patchNotKey] = surfaceVisitedMap[patchKey];
							//std::cout << "adding " << surfaceVisitedMap[patchKey] << " <-[5] " << patchNotKey << std::endl;
							//} else { std::cout << "already in " << surfaceVisitedMap[patchKey] << " <-[6] " << patchNotKey << std::endl; }
							continue;
						}

						// ... now check pathNotKey.
						if ( ((surfaceVisitedMapIterator = surfaceVisitedMap.find(patchNotKey)) != surfaceVisitedMap.end()) && 
							 ((surfaceVisitedMapIterator2 = surfaceVisitedMap.find(patchKey)) == surfaceVisitedMap.end()) ) {
							//if (((*surfacePatches)[surfaceVisitedMap[patchNotKey]]->size() > (pointsInContact * 100)) && 
							//	((*surfacePatches)[patchKey]->size() > (pointsInContact * 100))) continue;				

			
							surfacePatchGroups[surfaceVisitedMap[patchNotKey]]->push_back(patchKey);
							surfaceVisitedMap[patchKey] = surfaceVisitedMap[patchNotKey];
							//std::cout << "adding " << surfaceVisitedMap[patchNotKey] << " <-[7] " << patchKey << std::endl;
							//} else { std::cout << "already in " << surfaceVisitedMap[patchNotKey] << " <-[8] " << patchKey << std::endl; }
							continue;
						}	

						//finally if both are on a group we need to merge the two groups.
						if ( ((surfaceVisitedMapIterator = surfaceVisitedMap.find(patchNotKey)) != surfaceVisitedMap.end()) && 
							 ((surfaceVisitedMapIterator2 = surfaceVisitedMap.find(patchKey)) != surfaceVisitedMap.end()) ) {
							 if (surfaceVisitedMap[patchKey] == surfaceVisitedMap[patchNotKey]) continue; //they are already in the same group
							 if (((*surfacePatches)[surfaceVisitedMap[patchKey]]->size()) > ((*surfacePatches)[surfaceVisitedMap[patchNotKey]]->size())) {
								//merge patchNotKey into patchKey
								int gsize = surfacePatchGroups[surfaceVisitedMap[patchNotKey]]->size();
								int mergegroup = surfaceVisitedMap[patchNotKey];
								//std::cout << "Merging " << mergegroup << " group into " << surfaceVisitedMap[patchKey] << "iteration size = " << gsize << std::endl;
								for (int i = 0; i<gsize; ++i) {
									//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchKey] << " group " << std::endl;
									surfacePatchGroups[surfaceVisitedMap[patchKey]]->push_back(surfacePatchGroups[mergegroup]->at(i));
									//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchKey] << std::endl;
									surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = surfaceVisitedMap[patchKey];
								}
								surfacePatchGroups[surfaceVisitedMap[patchKey]]->push_back(mergegroup);
								surfacePatchGroups.erase(mergegroup);								
							 } else { 
								//merge patchKey into patchNotKey
								int gsize = surfacePatchGroups[surfaceVisitedMap[patchKey]]->size();
								int mergegroup = surfaceVisitedMap[patchKey];
								//std::cout << "Merging " << mergegroup << " group into " << surfaceVisitedMap[patchNotKey] << "iteration size = " << gsize << std::endl;
								for (int i = 0; i<gsize; ++i) {
									//std::cout << "Adding " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchNotKey] << " group " << std::endl;
									surfacePatchGroups[surfaceVisitedMap[patchNotKey]]->push_back(surfacePatchGroups[mergegroup]->at(i));
									//std::cout << "Setting visited map value of " << surfacePatchGroups[mergegroup]->at(i) << " to " << surfaceVisitedMap[patchNotKey] << std::endl;
									surfaceVisitedMap[surfacePatchGroups[mergegroup]->at(i)] = surfaceVisitedMap[patchNotKey];
								}
								surfacePatchGroups[surfaceVisitedMap[patchNotKey]]->push_back(mergegroup);
								surfacePatchGroups.erase(mergegroup);							
							 }
							 continue;
						}

						//Hopefully there are no more conditions to consider !!!!!!

					}

				//Now go through the original surface patches and see whether there were any without contacts and add.
				for (surfacePatchesIterator = surfacePatches->begin(); surfacePatchesIterator != surfacePatches->end(); ++surfacePatchesIterator) {
					if ((surfaceVisitedMapIterator = surfaceVisitedMap.find(surfacePatchesIterator->first)) == surfaceVisitedMap.end()) {
						if ((surfacePatchGroupsIterator = surfacePatchGroups.find(surfacePatchesIterator->first)) == surfacePatchGroups.end()) {
							surfacePatchGroups[surfacePatchesIterator->first] = new std::vector<int>();
						}
					}									
				}
#pragma endregion

#pragma region
				//Print the merged patches and populate map with reverse mapping information
				std::map<int, int> idmaps = std::map<int, int>();
				std::cout << "[SegmentationGraph] Revised Patch Count ->" << " S[" << surfacePatchGroups.size() << "]" << std::endl; 
				for (surfacePatchGroupsIterator = surfacePatchGroups.begin(); 
					 surfacePatchGroupsIterator != surfacePatchGroups.end(); ++surfacePatchGroupsIterator) {
						 //std::cout << surfacePatchGroupsIterator->first << " -> ";
						 idmaps[surfacePatchGroupsIterator->first] = surfacePatchGroupsIterator->first;
						 for (int i = 0; i<surfacePatchGroupsIterator->second->size(); i++) {
							 //std::cout << surfacePatchGroupsIterator->second->at(i) << " ";
							 idmaps[surfacePatchGroupsIterator->second->at(i)] = surfacePatchGroupsIterator->first;
						 }
						 //std::cout << std::endl;
				}

				//update the point cloud with the new surface patch Ids
				std::vector<size_t>::iterator pointIterator;
				for (cellClosureIterator = map->begin(); cellClosureIterator != map->end(); cellClosureIterator++) { 			
					for (pointIterator = cellClosureIterator->second->begin(); pointIterator != cellClosureIterator->second->end(); ++pointIterator) {
						pointCloud->GetPointAt(*pointIterator)->pcpId=idmaps[pointCloud->GetPointAt(*pointIterator)->pcpId];
					}
				}

#pragma endregion
								

				//Finally create,initialise and populate the graph of patches ... using boost::adjacency_list
				/*patchesGraph = new std::vector<std::vector<Point*>*>();								
				patchesGraph->resize(patch_id); //clean the patches first
				std::vector<std::vector<Point*>*> & patchesRef = *patches;
				for (int i = 0; i<patch_id; ++i) {
					patchesRef[i] = new std::vector<Point*>();
				}

				for (std::map<std::string, std::vector<Point*>*>::iterator cellIterator = map->begin(); 
					 cellIterator != map->end(); ++cellIterator)
				{

				}*/

				std::cout << boost::str(boost::format("[SegmentationGraph] Merging complete in %d seconds") % timer_merging.elapsed()) << std::endl;

				#pragma region deallocate heap memory used
				region.flush();
				delete same_patch_work_queue;
				delete different_patch_work_queue;
				delete neighbour_patches_queue;
				delete edge2edgeContacts;
				delete surface2surfaceContacts;
				delete surface2edgeContacts;
				
				#pragma endregion

			} // Build end


			
			
		};		

	}
}